﻿#include <xy/geom/nurbs.h>

namespace xy
{

namespace geom
{

using namespace xy::math;

// 导出模板函数
#define NURBS_EXT(T)                                                                                                   \
    template T horner1(const T *cpts, int n, double u);                                                                \
    template T horner2(const T *cpts, int n, int m, double u, double v);                                               \
    template T bezier_point(const T *cpts, int n, double u);                                                           \
    template T de_casteljau1(const T *cpts, int n, double u);                                                          \
    template T de_casteljau2(const T *cpts, int n, int m, double u, double v);                                         \
    template std::vector<T> bezier_elevate(const T *cpts, int n, int t);                                               \
    template double bezier_reduce(const T *cpts, int n, T *Q);                                                         \
    template void power_to_bezier(const T *cpts, int n, T *R);                                                         \
    template T curve_point(const T *cpts, const double *knots, int n, int p, double u);                                \
    template T surface_point(const T *cpts, const double *uknots, const double *vknots, int n, int m, int p, int q,    \
                             double u, double v);                                                                      \
    template std::vector<T> curve_deriv_alg1(const T *cpts, const double *knots, int n, int p, int d, double u);       \
    template std::vector<T> curve_deriv_alg2(const T *cpts, const double *knots, int n, int p, int d, double u);       \
    template std::vector<std::vector<T>> curve_deriv_cpts(const T *cpts, const double *knots, int n, int p, int d,     \
                                                          int r1, int r2);                                             \
    template std::vector<std::vector<T>> surface_deriv_alg1(const T *cpts, const double *uknots, const double *vknots, \
                                                            int n, int m, int p, int q, int d, double u, double v);    \
    template void curve_knot_ins(const T *cpts, const double *knots, int n, int p, double u, int k, int s, int r,      \
                                 T *Q, double *U);                                                                     \
    template void surface_knot_ins<T, 0>(const T *cpts, const double *knots, int n, int m, int p, int q, double uv,    \
                                         int k, int s, int r, T *Q, double *UV);                                       \
    template void surface_knot_ins<T, 1>(const T *cpts, const double *knots, int n, int m, int p, int q, double uv,    \
                                         int k, int s, int r, T *Q, double *UV);                                       \
    template T curve_point_by_corner_cut(const T *cpts, const double *knots, int n, int p, double u);                  \
    template void curve_knot_refine(const T *cpts, const double *knots, int n, int p, const double *x, int r, T *Q,    \
                                    double *U);                                                                        \
    template void surface_knot_refine<T, 0>(const T *cpts, const double *knots, int n, int m, int p, int q,            \
                                            const double *x, int r, T *Q, double *UV);                                 \
    template void surface_knot_refine<T, 1>(const T *cpts, const double *knots, int n, int m, int p, int q,            \
                                            const double *x, int r, T *Q, double *UV);                                 \
    template std::vector<std::vector<T>> curve_decompose(const T *cpts, const double *knots, int n, int p);            \
    template int curve_knot_remove<T, false>(T * cpts, double *knots, int n, int p, double u, int k, int s, int r,     \
                                             double tol);                                                              \
    template int curve_knot_remove<T, true>(T * cpts, double *knots, int n, int p, double u, int k, int s, int r,      \
                                            double tol);                                                               \
    template int curve_elevate(const T *cpts, const double *knots, int n, int p, int t, T *Q, double *U, int step1,    \
                               int step2);                                                                             \
    template int curve_reduce(const T *cpts, const double *knots, int n, int p, T *Q, double *U, double tol);          \
    template std::vector<double> equally_param(const std::vector<T> &points);                                          \
    template std::vector<double> chord_param(const std::vector<T> &points);                                            \
    template std::vector<double> centripetal_param(const std::vector<T> &points);                                      \
    template std::vector<std::vector<vec2>> equally_param(const std::vector<std::vector<T>> &points);                  \
    template std::vector<std::vector<vec2>> chord_param(const std::vector<std::vector<T>> &points);                    \
    template std::vector<std::vector<vec2>> centripetal_param(const std::vector<std::vector<T>> &points);

NURBS_EXT(vec2)
NURBS_EXT(vec3)
NURBS_EXT(vec4)

#undef NURBS_EXT

#define NURBS_EXT(N, T)                                                                                                \
    template std::vector<vec<N, T>> rat_curve_deriv(const vec<N + 1, T> *cpts, const double *knots, int n, int p,      \
                                                    int d, double u);                                                  \
    template std::vector<std::vector<vec<N, T>>> rat_surface_deriv(const vec<N + 1, T> *cpts, const double *uknots,    \
                                                                   const double *vknots, int n, int m, int p, int q,   \
                                                                   int d, double u, double v);

NURBS_EXT(2, double)
NURBS_EXT(3, double)

#undef NURBS_EXT

template <class T> T horner1(const T *cpts, int n, double u)
{
    T res = cpts[n - 1];
    for (int i = n - 2; i >= 0; --i)
        res = res * u + cpts[i];
    return res;
}

template <class T> T horner2(const T *cpts, int n, int m, double u, double v)
{
    std::vector<T> b(n);
    for (int i = 0; i < n; i++)
        b[i] = horner1(cpts + i * m, m, v);
    return horner1(b.data(), n, u);
}

double bernstein(int n, int i, double u)
{
    std::vector<double> b(n + 1, 0);
    b[n - i] = 1;

    double u1 = 1 - u;
    for (int k = 1; k <= n; k++)
        for (int j = n; j >= k; j--)
            b[j] = u1 * b[j] + u * b[j - 1];

    return b[n];
}

std::vector<double> all_bernstein(int n, double u)
{
    std::vector<double> b(n + 1, 1);
    double u1 = 1 - u;

    for (int j = 1; j <= n; j++)
    {
        double saved = 0;
        for (int k = 0; k < j; k++)
        {
            double temp = b[k];
            b[k] = saved + u1 * temp;
            saved = u * temp;
        }
        b[j] = saved;
    }

    return b;
}

template <class T> T bezier_point(const T *cpts, int n, double u)
{
    auto b = all_bernstein(n - 1, u);
    T res{};
    for (int i = 0; i < n; i++)
        res += cpts[i] * b[i];
    return res;
}

template <class T> T de_casteljau1(const T *cpts, int n, double u)
{
    double u1 = 1 - u;
    std::vector<T> Q(n);
    std::copy(cpts, cpts + n, Q.begin());
    for (int k = 1; k <= n; k++)
        for (int i = 0; i < n - k; i++)
            Q[i] = Q[i] * u1 + Q[i + 1] * u;
    return Q[0];
}

template <class T> T de_casteljau2(const T *cpts, int n, int m, double u, double v)
{
    std::vector<T> Q(n);
    for (int i = 0; i < n; i++)
        Q[i] = de_casteljau1(cpts + i * m, m, v);
    return de_casteljau1(Q.data(), n, u);
}

template <class T> std::vector<T> bezier_elevate(const T *cpts, int n, int t)
{
    int p = n - 1;
    int ph = p + t;
    int ph2 = ph / 2;

    std::vector<T> Q(ph + 1, T{});
    std::vector<std::vector<double>> alpha(ph + 1, std::vector<double>(p + 1));
    alpha[0][0] = alpha[ph][p] = 1.0;

    // 计算新的控制点 P_i^t 对应的系数 alpha[i][j] * P_j
    for (int i = 1; i < ph2 + 1; i++)
    {
        double inv = 1.0 / binomial(ph, i);
        int mpi = std::min(p, i);
        for (int j = std::max(0, i - t); j < mpi + 1; j++)
            alpha[i][j] = inv * binomial(p, j) * binomial(t, i - j);
    }

    // 由于二项式对称性，只需要计算一半，然后 copy 到另一半
    for (int i = ph2 + 1; i < ph; i++)
    {
        int mpi = std::min(p, i);
        for (int j = std::max(0, i - t); j < mpi + 1; j++)
            alpha[i][j] = alpha[ph - i][p - j];
    }

    // 计算新的控制点 P_i^t
    for (int i = 0; i < ph + 1; i++)
    {
        int mpi = std::min(p, i);
        for (int j = std::max(0, i - t); j < mpi + 1; j++)
            Q[i] += cpts[j] * alpha[i][j];
    }

    return Q;
}

template <class T> double bezier_reduce(const T *cpts, int n, T *Q)
{
    int p = n - 1;

    Q[0] = cpts[0];
    Q[p - 1] = cpts[p];

    int r = (p - 1) / 2;
    for (int i = 1; i < r + 1; i++)
    {
        double alpha = double(i) / p;
        Q[i] = (cpts[i] - Q[i - 1] * alpha) / (1 - alpha);
    }

    for (int i = p - 2; i >= r + 1; i--)
    {
        double alpha = double(i) / p;
        Q[i] = (cpts[i + 1] - Q[i + 1] * (1 - alpha)) / alpha;
    }

    // 如果 p 为奇数，中间的控制点通过左右平均得到
    double tol = 0;
    if (p % 2 == 1)
    {
        double alpha = double(r) / p;
        T tmp = (cpts[r + 1] - Q[r + 1] * (1 - alpha)) / alpha;
        tol = (tmp - Q[r]).modulus();
        Q[r] = (Q[r] + tmp) / 2;
    }
    else
        tol = (cpts[r + 1] - (Q[r] + Q[r + 1]) / 2).modulus();

    return tol;
}

template <class T> void power_to_bezier(const T *cpts, int n, T *R)
{
    // R_i^k = P_{n-k} + i / k * R_{i-1}^{k-i}
    // i = 0, ..., k, k = 1, ..., n, R_0 = P_n
    n = n - 1;
    R[0] = cpts[n];
    for (int k = 1; k < n + 1; k++)
    {
        for (int i = k; i > 0; i--)
            R[i] = cpts[n - k] + R[i - 1] * i / k;
        R[0] = cpts[n - k];
    }
}

void bezier_to_power_matrix(int p, double *M)
{
    int col = p + 1;
    for (int i = 0; i < p; i++)
        for (int j = i + 1; j < p + 1; j++)
            M[i * col + j] = 0;
    M[0] = M[p * col + p] = 1;

    // 左下角
    if (p % 2 == 1)
        M[p * col] = -1;
    else
        M[p * col] = 1;

    // 计算第一列和最后一行
    auto sign = -1;
    for (int i = 1; i < p; i++)
    {
        M[i * col + i] = binomial(p, i);
        M[i * col] = M[p * col + p - i] = sign * M[i * col + i];
        sign *= -1;
    }

    // 计算剩余元素
    auto k1 = (p + 1) / 2;
    auto pk = p - 1;
    for (int k = 1; k < k1; k++)
    {
        sign = -1;
        for (int j = k + 1; j < pk + 1; j++)
        {
            M[j * col + k] = M[pk * col + p - j] = sign * binomial(p, k) * binomial(p - k, j - k);
            sign *= -1;
        }
        pk--;
    }
}

void power_to_bezier_matrix(int p, const double *M, double *MI)
{
    int col = p + 1;
    for (int i = 0; i < p; i++)
        for (int j = i + 1; j < p + 1; j++)
            MI[i * col + j] = 0;

    for (int i = 0; i < p + 1; i++)
    {
        MI[i * col] = MI[p * col + i] = 1;
        MI[i * col + i] = 1 / M[i * col + i];
    }

    auto k1 = (p + 1) / 2;
    auto pk = p - 1;
    for (int k = 1; k < k1; k++)
    {
        for (int j = k + 1; j < pk + 1; j++)
        {
            double d = 0;
            for (int i = k; i < j; i++)
                d -= M[j * col + i] * MI[i * col + k];
            MI[j * col + k] = d / M[j * col + j];
            MI[pk * col + p - j] = MI[j * col + k];
        }
        pk--;
    }
}

int find_span(const double *knots, int n, int p, double u)
{
    // 返回左闭右开区间 u \in [u_i, u_{i+1})
    // 边界情况：如果 u 在区间最大值的边界上，就返回左边的区间
    // 注意这里边界是最大的值，而不是节点最大值
    if (std::abs(u - knots[n - p - 1]) < std::numeric_limits<double>::epsilon())
        return n - p - 2;

    // 二分法找到最后一个不超过 u 的 knot
    int low = p;
    int high = n - p - 1;
    int mid = (low + high) / 2;

    // 退出条件 u \in [u_mid, u_{mid+1})]
    while (u < knots[mid] || u >= knots[mid + 1])
    {
        if (u < knots[mid])
            high = mid;
        else
            low = mid;
        mid = (low + high) / 2;
    }
    return mid;
}

int find_mult_span(const double *knots, int i, int p, double u)
{
    // 由于查找时会找到最大的包含 u 的节点区间，因此需要向前搜索
    int r = 0;
    while (i >= p && std::abs(u - knots[i]) < std::numeric_limits<double>::epsilon())
        r++, i--;
    return r;
}

std::vector<double> basis_funcs(const double *knots, int p, int i, double u)
{
    std::vector<double> N(p + 1);     // 存放 u \in [u_i, u_{i+1}) 处不为零的基函数值
    std::vector<double> left(p + 1);  // left[j] = u - u_{i+1-j}
    std::vector<double> right(p + 1); // right[j] = u_{i+j} - u

    N[0] = 1;
    for (int j = 1; j < p + 1; j++)
    {
        left[j] = u - knots[i + 1 - j];
        right[j] = knots[i + j] - u;

        double saved = 0;
        for (int r = 0; r < j; r++)
        {
            double tmp = N[r] / (right[r + 1] + left[j - r]);
            N[r] = saved + right[r + 1] * tmp;
            saved = left[j - r] * tmp;
        }
        N[j] = saved;
    }
    return N;
}

std::vector<std::vector<double>> all_basis_funcs(const double *knots, int p, int i, double u)
{
    std::vector<std::vector<double>> N(p + 1); // 存放 u \in [u_i, u_{i+1}) 处不为零的基函数值
    std::vector<double> left(p + 1);           // left[j] = u - u_{i+1-j}
    std::vector<double> right(p + 1);          // right[j] = u_{i+j} - u

    N[0].resize(1);
    N[0][0] = 1;
    for (int j = 1; j < p + 1; j++)
    {
        N[j].resize(j + 1);
        left[j] = u - knots[i + 1 - j];
        right[j] = knots[i + j] - u;

        // N[j][r] = N_{r,j}，即 j 次 r 下标
        double saved = 0;
        for (int r = 0; r < j; r++)
        {
            double tmp = N[j - 1][r] / (right[r + 1] + left[j - r]);
            N[j][r] = saved + right[r + 1] * tmp;
            saved = left[j - r] * tmp;
        }
        N[j][j] = saved;
    }
    return N;
}

std::vector<std::vector<double>> deriv_basis_funcs(const double *knots, int p, int i, int d, double u)
{
    // ders[k][j] 表示 N_{i-p+j}^(k)
    std::vector<std::vector<double>> ders(d + 1, std::vector<double>(p + 1));

    // 先计算出非零基函数及节点差分
    std::vector<std::vector<double>> ndu(p + 1, std::vector<double>(p + 1));
    std::vector<double> left(p + 1);  // left[j] = u - u_{i+1-j}
    std::vector<double> right(p + 1); // right[j] = u_{i+j} - u

    ndu[0][0] = 1;
    for (int j = 1; j < p + 1; j++)
    {
        left[j] = u - knots[i + 1 - j];
        right[j] = knots[i + j] - u;

        double saved = 0;
        for (int r = 0; r < j; r++)
        {
            // 下三角部分存放节点差分
            // ndu[j][r] = u_{i + r + 1} - u_{i + r + 1 - j}
            ndu[j][r] = right[r + 1] + left[j - r];

            // 上三角部分（包括对角元）存放所有非零不大于 p 次基函数（basis_funcs）
            // 注意这里 ndu[r][j] = N_{r,j}，没有像 basis_funcs 一样反过来，因为空间已经分配好
            double tmp = ndu[r][j - 1] / ndu[j][r];
            ndu[r][j] = saved + right[r + 1] * tmp;
            saved = left[j - r] * tmp;
        }
        ndu[j][j] = saved;
    }

    // 将计算结果中的基函数部分（上三角）记录下来
    for (int j = 0; j < p + 1; j++)
        ders[0][j] = ndu[j][p];

    // N_{i,p}^(k) = p * [ N_{i,p-1}^(k-1) / (u_{i+p} - u_i) - N_{i+1,p-1}^(k-1) / (u_{i+p+1}-u_{i+1}) ]
    std::vector<std::vector<double>> a(2, std::vector<double>(p + 1));
    for (int r = 0; r < p + 1; r++)
    {
        // 计算 N_{i,r}^(k)
        int s1 = 0, s2 = 1;

        // a_{0,0} = 1
        a[0][0] = 1;
        for (int k = 1; k < d + 1; k++)
        {
            double der = 0;

            // a_{k,0} = a_{k-1,0} / (u_{i+p-k+1} - u_i)
            // 从 N_{i-p+r,p}^(k) 开始计算，这里 "i" 是 i-p+r
            // 所以取 ndu[p - k + 1][r - k] = u_{i+r-k+1} - u_{i-p+r+j}
            if (r >= k)
            {
                // a[s2] 存放 a_{k, }, a[s1] 存放 a_{k-1, }
                a[s2][0] = a[s1][0] / ndu[p - k + 1][r - k];
                der = a[s2][0] * ndu[r - k][p - k];
            }

            // 正常情况下 j1 = 1, j2 = k - 1，就是完整的计算求和
            // 特殊情况下要排除掉系数 a 为零和基函数 N 为零的情况
            int j1, j2;
            if (r - k >= -1)
                j1 = 1;
            else
                j1 = -r + k;

            if (r - 1 <= p - k)
                j2 = k - 1;
            else
                j2 = p - r;

            // a_{k,j} = (a_{k-1,j} - a_{k-1,j-1}) / (u_{i+p+j-k+1} - u_{i+j})
            // 注意这里 "i" 是 i-p+r
            // j = 1, ..., k-1
            for (int j = j1; j < j2 + 1; j++)
            {
                a[s2][j] = (a[s1][j] - a[s1][j - 1]) / ndu[p - k + 1][r - k + j];
                der += a[s2][j] * ndu[r - k + j][p - k];
            }

            // 计算对角 a_{k,k} = -a_{k-1,k-1} / (u_{i+p+1} - u_{i+k})
            // 注意这里 "i" 是 i-p+r
            if (r <= p - k)
            {
                a[s2][k] = -a[s1][k - 1] / ndu[p - k + 1][r];
                der += a[s2][k] * ndu[r][p - k];
            }
            ders[k][r] = der;

            // 交换行，将之前计算的 a_{k, } 作为下一个循环的 a_{k-1, }
            std::swap(s1, s2);
        }
    }

    // 由于重复求导，乘上因子 p * (p - 1) * ... * (p - k + 1)
    int r = p;
    for (int k = 1; k < d + 1; k++)
    {
        for (int j = 0; j < p + 1; j++)
            ders[k][j] *= r;
        r *= p - k;
    }
    return ders;
}

double one_basis_func(const double *knots, int n, int p, int i, double u)
{
    int m = n - 1;
    if ((i == 0 && u == knots[0]) || (i == m - p - 1 && u == knots[m]))
        return 1;

    if (u < knots[i] || u >= knots[i + p + 1])
        return 0;

    // 初始化 1 次基函数
    std::vector<double> N(p + 1);
    for (int j = 0; j < p + 1; j++)
    {
        if (u >= knots[i + j] && u < knots[i + j + 1])
            N[j] = 1;
        else
            N[j] = 0;
    }

    double saved = 0;
    for (int k = 1; k < p + 1; k++)
    {
        // 考虑边界情况
        if (N[0] == 0)
            saved = 0;
        else
            saved = ((u - knots[i]) * N[0]) / (knots[i + k] - knots[i]);

        for (int j = 0; j < p - k + 1; j++)
        {
            double Uleft = knots[i + j + 1];
            double Uright = knots[i + j + k + 1];
            if (N[j + 1] == 0)
            {
                N[j] = saved;
                saved = 0;
            }
            else
            {
                double tmp = N[j + 1] / (Uright - Uleft);
                N[j] = saved + (Uright - u) * tmp;
                saved = (u - Uleft) * tmp;
            }
        }
    }
    return N[0];
}

std::vector<double> deriv_one_basis_func(const double *knots, int n, int p, int i, int d, double u)
{
    int m = n - 1;
    std::vector<double> ders(d + 1, 0);
    if (u < knots[i] || u >= knots[i + p + 1])
        return ders;

    // 初始化 1 次基函数
    std::vector<std::vector<double>> N(p + 1, std::vector<double>(d + 1));
    for (int j = 0; j < p + 1; j++)
    {
        if (u >= knots[i + j] && u < knots[i + j + 1])
            N[j][0] = 1;
        else
            N[j][0] = 0;
    }

    // 计算所有基函数
    double saved = 0;
    for (int k = 1; k < p + 1; k++)
    {
        if (N[0][k - 1] == 0)
            saved = 0;
        else
            saved = ((u - knots[i]) * N[0][k - 1]) / (knots[i + k] - knots[i]);

        for (int j = 0; j < p - k + 1; j++)
        {
            double Uleft = knots[i + j + 1];
            double Uright = knots[i + j + k + 1];
            if (N[j + 1][k - 1] == 0)
            {
                N[j][k] = saved;
                saved = 0;
            }
            else
            {
                double tmp = N[j + 1][k - 1] / (Uright - Uleft);
                N[j][k] = saved + (Uright - u) * tmp;
                saved = (u - Uleft) * tmp;
            }
        }
    }

    // 递归计算导数
    ders[0] = N[0][p];
    for (int k = 1; k < d + 1; k++)
    {
        std::vector<double> ND(k + 1);
        for (int j = 0; j < k + 1; j++)
            ND[j] = N[j][p - k];

        for (int jj = 1; jj < k + 1; jj++)
        {
            if (ND[0] == 0)
                saved = 0;
            else
                saved = ND[0] / (knots[i + p - k + jj] - knots[i]);

            for (int kk = 0; kk < k - jj + 1; kk++)
            {
                double Uleft = knots[i + kk + 1];
                double Uright = knots[i + kk + p + jj + 1];
                if (ND[kk + 1] == 0)
                {
                    ND[kk] = (p - k + jj) * saved;
                    saved = 0;
                }
                else
                {
                    double tmp = ND[kk + 1] / (Uright - Uleft);
                    ND[kk] = (p - k + jj) * (saved - tmp);
                    saved = tmp;
                }
            }
        }
        ders[k] = ND[0];
    }

    return ders;
}

template <class T> T curve_point(const T *cpts, const double *knots, int n, int p, double u)
{
    auto span = find_span(knots, n, p, u);
    auto basis = basis_funcs(knots, p, span, u);

    T pt{};
    for (int i = 0; i < p + 1; i++)
        pt += cpts[span - p + i] * basis[i];
    return pt;
}

template <class T>
T surface_point(const T *cpts, const double *uknots, const double *vknots, int n, int m, int p, int q, double u,
                double v)
{
    auto cols = m - q - 1;
    auto uspan = find_span(uknots, n, p, u);
    auto vspan = find_span(vknots, m, q, v);
    auto ubasis = basis_funcs(uknots, p, uspan, u);
    auto vbasis = basis_funcs(vknots, q, vspan, v);

    T pt{};
    for (int i = 0; i < p + 1; i++)
        for (int j = 0; j < q + 1; j++)
            pt += cpts[(uspan - p + i) * cols + vspan - q + j] * ubasis[i] * vbasis[j];
    return pt;
}

template <class T> std::vector<T> curve_deriv_alg1(const T *cpts, const double *knots, int n, int p, int d, double u)
{
    int du = std::min(d, p);
    std::vector<T> ders(d + 1, T{});

    auto span = find_span(knots, n, p, u);
    auto basis = deriv_basis_funcs(knots, p, span, du, u);

    for (int i = 0; i < du + 1; i++)
        for (int j = 0; j < p + 1; j++)
            ders[i] += cpts[span - p + j] * basis[i][j];

    return ders;
}

template <class T> std::vector<T> curve_deriv_alg2(const T *cpts, const double *knots, int n, int p, int d, double u)
{
    int du = std::min(d, p);
    std::vector<T> ders(d + 1, T{});

    auto span = find_span(knots, n, p, u);
    auto basis = all_basis_funcs(knots, p, span, u);
    auto PK = curve_deriv_cpts(cpts, knots, n, p, du, span - p, span);

    for (int k = 0; k < du + 1; k++)
        for (int j = 0; j < p - k + 1; j++)
            ders[k] += PK[k][j] * basis[p - k][j];

    return ders;
}

template <int N, class T>
std::vector<vec<N - 1, T>> rat_curve_deriv(const vec<N, T> *cpts, const double *knots, int n, int p, int d, double u)
{
    // 直接计算导数
    auto ders = curve_deriv_alg1(cpts, knots, n, p, d, u);

    std::vector<vec<N - 1, T>> CK(d + 1);
    for (int k = 0; k < d + 1; k++)
    {
        // 取前 N-1 位
        vec<N - 1, T> v;
        std::copy(ders[k].data(), ders[k].data() + N - 1, v.data());

        for (int i = 1; i < k + 1; i++)
            v -= CK[k - i] * ders[i][N - 1] * binomial(k, i);
        CK[k] = v / ders[0][N - 1];
    }
    return CK;
}

template <class T>
std::vector<std::vector<T>> curve_deriv_cpts(const T *cpts, const double *knots, int n, int p, int d, int r1, int r2)
{
    int r = r2 - r1;
    std::vector<std::vector<T>> PK(d + 1, std::vector<T>(r + 1));
    for (int i = 0; i < r + 1; i++)
        PK[0][i] = cpts[r1 + i];

    for (int k = 1; k < d + 1; k++)
    {
        int tmp = p - k + 1;
        for (int i = 0; i < r - k + 1; i++)
            PK[k][i] = (PK[k - 1][i + 1] - PK[k - 1][i]) / (knots[r1 + i + p + 1] - knots[r1 + i + k]) * tmp;
    }
    return PK;
}

template <class T>
std::vector<std::vector<T>> surface_deriv_alg1(const T *cpts, const double *uknots, const double *vknots, int n, int m,
                                               int p, int q, int d, double u, double v)
{
    int du = std::min(d, p);
    int dv = std::min(d, q);

    auto uspan = find_span(uknots, n, p, u);
    auto vspan = find_span(vknots, m, q, v);
    auto ubasis = deriv_basis_funcs(uknots, p, uspan, du, u);
    auto vbasis = deriv_basis_funcs(vknots, q, vspan, dv, v);

    std::vector<std::vector<T>> ders(d + 1, std::vector<T>(d + 1, T{}));
    for (int k = 0; k < du + 1; k++)
    {
        std::vector<T> tmp(q + 1, T{});
        for (int s = 0; s < q + 1; s++)
            for (int r = 0; r < p + 1; r++)
                tmp[s] += cpts[(uspan - p + r) * (m - q - 1) + vspan - q + s] * ubasis[k][r];

        int dd = std::min(d - k, dv);
        for (int l = 0; l < dd + 1; l++)
            for (int s = 0; s < q + 1; s++)
                ders[k][l] += tmp[s] * vbasis[l][s];
    }

    return ders;
}

template <int N, class T>
std::vector<std::vector<vec<N - 1, T>>> rat_surface_deriv(const vec<N, T> *cpts, const double *uknots,
                                                          const double *vknots, int n, int m, int p, int q, int d,
                                                          double u, double v)
{
    // 直接计算导数
    auto ders = surface_deriv_alg1(cpts, uknots, vknots, n, m, p, q, d, u, v);

    std::vector<std::vector<vec<N - 1, T>>> SKL(d + 1, std::vector<vec<N - 1, T>>(d + 1));
    for (int k = 0; k < d + 1; k++)
    {
        for (int l = 0; l < d - k + 1; l++)
        {
            // 取前 N-1 位
            vec<N - 1, T> v;
            std::copy(ders[k][l].data(), ders[k][l].data() + N - 1, v.data());

            for (int j = 1; j < l + 1; j++)
                v -= SKL[k][l - j] * ders[0][j][N - 1] * binomial(l, j);

            for (int i = 1; i < k + 1; i++)
            {
                v -= SKL[k - i][l] * ders[i][0][N - 1] * binomial(k, i);

                vec<N - 1, T> v2;
                for (int j = 1; j < l + 1; j++)
                    v2 += SKL[k - i][l - j] * ders[i][j][N - 1] * binomial(l, j);

                v -= v2 * binomial(k, i);
            }

            SKL[k][l] = v / ders[0][0][N - 1];
        }
    }
    return SKL;
}

template <class T>
void curve_knot_ins(const T *cpts, const double *knots, int n, int p, double u, int k, int s, int r, T *Q, double *U)
{
    int np = n - p - 2; // 控制点数 -1
    int mp = n - 1;     // 节点数 -1

    // 保存不需要动的控制点
    for (int i = 0; i < k - p + 1; i++)
        Q[i] = cpts[i];
    for (int i = k - s; i < np + 1; i++)
        Q[i + r] = cpts[i];

    // 保存用于迭代的控制点 k-p 到 k-s
    std::vector<T> R(p + 1);
    for (int i = 0; i < p - s + 1; i++)
        R[i] = cpts[k - p + i];

    // 循环插入
    for (int j = 1; j < r + 1; j++)
    {
        int L = k - p + j;

        // k - p + r <= i <= k - s
        for (int i = 0; i < p - j - s + 1; i++)
        {
            double alpha = (u - knots[L + i]) / (knots[i + k + 1] - knots[L + i]);
            R[i] = R[i + 1] * alpha + R[i] * (1 - alpha);
        }

        // 顶部和底部两个控制点不变
        Q[L] = R[0];
        Q[k - s + r - j] = R[p - s - j];
    }

    // 存储更新的控制点
    int L = k - p + r;
    for (int i = L + 1; i < k - s; i++)
        Q[i] = R[i - L];

    // 插入新的节点后的节点向量
    for (int i = 0; i < k + 1; i++)
        U[i] = knots[i];
    for (int i = 1; i < r + 1; i++)
        U[k + i] = u;
    for (int i = k + 1; i < mp + 1; i++)
        U[i + r] = knots[i];
}

template <class T, int D>
void surface_knot_ins(const T *cpts, const double *knots, int n, int m, int p, int q, double uv, int k, int s, int r,
                      T *Q, double *UV)
{
    int np = n - p - 2; // u 控制点数 -1
    int mp = m - q - 2; // v 控制点数 -1

    // 根据插入方向调整映射关系
    auto id1 = [=](int i, int j) {
        if constexpr (D == 0)
            return i * (mp + 1) + j;
        else
            return j * (mp + 1) + i;
    };

    auto id2 = [=](int i, int j) {
        if constexpr (D == 0)
            return i * (mp + 1) + j;
        else
            return j * (mp + r + 1) + i;
    };

    if constexpr (D == 1)
    {
        std::swap(n, m);
        std::swap(p, q);
        std::swap(np, mp);
    }

    // 保存 alpha，只需要计算一次，对应每个下标和插入次数
    std::vector<std::vector<double>> alpha(p - s, std::vector<double>(r + 1));
    for (int j = 1; j < r + 1; j++)
    {
        int L = k - p + j;
        for (int i = 0; i < p - s + 1 - j; i++)
            alpha[i][j] = (uv - knots[L + i]) / (knots[i + k + 1] - knots[L + i]);
    }

    // 开始对每一行计算控制点
    for (int row = 0; row < mp + 1; row++)
    {
        // 保存不需要动的控制点
        for (int i = 0; i < k - p + 1; i++)
            Q[id2(i, row)] = cpts[id1(i, row)];
        for (int i = k - s; i < np + 1; i++)
            Q[id2(i + r, row)] = cpts[id1(i, row)];

        // 保存用于迭代的控制点 k-p 到 k-s
        std::vector<T> R(p + 1);
        for (int i = 0; i < p - s + 1; i++)
            R[i] = cpts[id1(k - p + i, row)];

        // 循环插入
        for (int j = 1; j < r + 1; j++)
        {
            int L = k - p + j;

            // k - p + r <= i <= k - s
            for (int i = 0; i < p - j - s + 1; i++)
                R[i] = R[i + 1] * alpha[i][j] + R[i] * (1 - alpha[i][j]);

            // 顶部和底部两个控制点不变
            Q[id2(L, row)] = R[0];
            Q[id2(k - s + r - j, row)] = R[p - s - j];
        }

        // 存储更新的控制点
        int L = k - p + r;
        for (int i = L + 1; i < k - s; i++)
            Q[id2(i, row)] = R[i - L];
    }

    // 插入新的节点后的节点向量
    for (int i = 0; i < k + 1; i++)
        UV[i] = knots[i];
    for (int i = 1; i < r + 1; i++)
        UV[k + i] = uv;
    for (int i = k + 1; i < n; i++)
        UV[i + r] = knots[i];
}

template <class T> T curve_point_by_corner_cut(const T *cpts, const double *knots, int n, int p, double u)
{
    if (u == knots[0])
        return cpts[0];
    if (u == knots[n])
        return cpts[n - p - 1];

    int span = find_span(knots, n, p, u);
    int r = find_mult_span(knots, span, p, u);
    r = p - r;

    // 保存影响范围内的控制点
    std::vector<T> R(r + 1);
    for (int i = 0; i < r + 1; i++)
        R[i] = cpts[span - p + i];

    for (int j = 1; j < r + 1; j++)
    {
        for (int i = 0; i < r + 1 - j; i++)
        {
            // 割线计算
            double alpha = (u - knots[span - p + j + i]) / (knots[span + i + 1] - knots[span - p + j + i]);
            R[i] = R[i + 1] * alpha + R[i] * (1 - alpha);
        }
    }
    return R[0];
}

template <class T>
void curve_knot_refine(const T *cpts, const double *knots, int n, int p, const double *x, int r, T *Q, double *U)
{
    int nq = n - 1;      // 节点数 -1
    int np = nq - p - 1; // 控制点数 -1
    r = r - 1;           // 插入点数 -1

    auto a = find_span(knots, nq + 1, p, x[0]);
    auto b = find_span(knots, nq + 1, p, x[r]) + 1;

    // 保存不需要动的控制点
    for (int j = 0; j < a - p + 1; j++)
        Q[j] = cpts[j];
    for (int j = b - 1; j < np + 1; j++)
        Q[j + r + 1] = cpts[j];

    // 保存新的节点
    for (int j = 0; j < a + 1; j++)
        U[j] = knots[j];
    for (int j = b + p; j < nq + 1; j++)
        U[j + r + 1] = knots[j];

    int i = b + p - 1;
    int k = b + p + r;
    for (int j = r; j >= 0; j--)
    {
        while (x[j] <= knots[i] && i > a)
        {
            U[k] = knots[i];
            Q[k - p - 1] = cpts[i - p - 1];

            k--;
            i--;
        }
        Q[k - p - 1] = Q[k - p];

        for (int l = 1; l < p + 1; l++)
        {
            int ind = k - p + l;
            double alpha = U[k + l] - x[j];
            if (std::abs(alpha) == 0)
                Q[ind - 1] = Q[ind];
            else
            {
                alpha /= U[k + l] - knots[i - p + l];
                Q[ind - 1] = Q[ind - 1] * alpha + Q[ind] * (1 - alpha);
            }
        }
        U[k] = x[j];
        k--;
    }
}

template <class T, int D>
void surface_knot_refine(const T *cpts, const double *knots, int n, int m, int p, int q, const double *x, int r, T *Q,
                         double *UV)
{
    int nq = n - 1;      // u 节点数 -1
    int np = nq - p - 1; // u 控制点数 -1
    int mq = m - 1;      // v 节点数 -1
    int mp = mq - q - 1; // v 控制点数 -1
    r = r - 1;           // 插入点数 -1

    // 根据插入方向调整映射关系
    auto id1 = [=](int i, int j) {
        if constexpr (D == 0)
            return i * (mp + 1) + j;
        else
            return j * (mp + 1) + i;
    };

    auto id2 = [=](int i, int j) {
        if constexpr (D == 0)
            return i * (mp + 1) + j;
        else // 注意 r 之前减了 1，这里要加上 1
            return j * (mp + r + 2) + i;
    };

    if constexpr (D == 1)
    {
        std::swap(n, m);
        std::swap(p, q);
        std::swap(np, mp);
        std::swap(nq, mq);
    }

    auto a = find_span(knots, nq + 1, p, x[0]);
    auto b = find_span(knots, nq + 1, p, x[r]) + 1;

    // 保存不需要动的控制点
    for (int row = 0; row < mp + 1; row++)
    {
        for (int j = 0; j < a - p + 1; j++)
            Q[id2(j, row)] = cpts[id1(j, row)];
        for (int j = b - 1; j < np + 1; j++)
            Q[id2(j + r + 1, row)] = cpts[id1(j, row)];
    }

    // 保存新的节点
    for (int j = 0; j < a + 1; j++)
        UV[j] = knots[j];
    for (int j = b + p; j < nq + 1; j++)
        UV[j + r + 1] = knots[j];

    int i = b + p - 1;
    int k = b + p + r;
    for (int j = r; j >= 0; j--)
    {
        while (x[j] <= knots[i] && i > a)
        {
            UV[k] = knots[i];
            for (int row = 0; row < mp + 1; row++)
                Q[id2(k - p - 1, row)] = cpts[id1(i - p - 1, row)];

            k--;
            i--;
        }
        for (int row = 0; row < mp + 1; row++)
            Q[id2(k - p - 1, row)] = Q[id2(k - p, row)];

        for (int l = 1; l < p + 1; l++)
        {
            int ind = k - p + l;
            for (int row = 0; row < mp + 1; row++)
            {
                double alpha = UV[k + l] - x[j];
                if (std::abs(alpha) == 0)
                    Q[id2(ind - 1, row)] = Q[id2(ind, row)];
                else
                {
                    alpha /= UV[k + l] - knots[i - p + l];
                    Q[id2(ind - 1, row)] = Q[id2(ind - 1, row)] * alpha + Q[id2(ind, row)] * (1 - alpha);
                }
            }
        }
        UV[k] = x[j];
        k--;
    }
}

template <class T> std::vector<std::vector<T>> curve_decompose(const T *cpts, const double *knots, int n, int p)
{
    int nq = n - 1;      // 节点数 -1
    int np = nq - p - 1; // 控制点数 -1

    int a = p;     // 跳过前面 p 个节点，到达有效区间的起点
    int b = p + 1; // 记录当前节点位置
    int nb = 0;

    std::vector<std::vector<T>> Q(nq - b, std::vector<T>(p + 1));
    for (int i = 0; i < p + 1; i++)
        Q[nb][i] = cpts[i];

    while (b < nq)
    {
        // 跳过重节点，计算重数
        int i = b;
        while (b < nq && knots[b + 1] == knots[b])
            b++;

        int mult = b - i + 1;
        if (mult < p)
        {
            // 插入 ub 节点
            double numer = knots[b] - knots[a];
            std::vector<double> alphas(p - mult);
            for (int j = p; j > mult; j--)
                alphas[j - mult - 1] = numer / (knots[a + j] - knots[a]);

            // 开始插入节点
            int r = p - mult;
            for (int j = 1; j < r + 1; j++)
            {
                int save = r - j;
                int s = mult + j;

                // 计算新的控制点
                for (int k = p; k >= s; k--)
                {
                    double alpha = alphas[k - s];
                    Q[nb][k] = Q[nb][k] * alpha + Q[nb][k - 1] * (1 - alpha);
                }

                // 将当前 Bezier 段的最后一个控制点作为下一段 Bezier 的控制点
                // alpha_{b-p+1,1} = alpha_{b-p+2,2} = ... = alpha_{b-s,p-s}
                // alpha_{b-p+2,1} = alpha_{b-p+3,2} = ...
                // alpha_{b-p+3,1}
                //...
                if (b < nq)
                    Q[nb + 1][save] = Q[nb][p];
            }
        }

        // 此时 b 到达下一个有效区间的起点
        if (b < nq)
        {
            // 读取下一个有效区间中的控制点
            for (int i = 0; i < mult + 1; i++)
                Q[nb + 1][i + p - mult] = cpts[b + i - mult];

            // 更新 a 为区间起点
            a = b;
            b++;
        }
        nb++;
    }
    Q.resize(nb);

    return Q;
}

template <class T, bool Rat>
int curve_knot_remove(T *cpts, double *knots, int n, int p, double u, int k, int s, int r, double tol)
{
    int np = n - p - 2; // 控制点数 -1
    int nq = n - 1;     // 节点数 -1

    int first = k - p; // 第一个控制点的下标
    int last = k - s;  // 最后一个控制点的下标

    // 临时保存新的控制点
    std::vector<T> tmp(2 * p + 1);

    // 计算容差
    double d = 0;
    for (int i = 0; i < np + 1; i++)
        d = std::max(d, homogeneous_to_cartesian(cpts[i]).modulus());

    double w = 1;
    if constexpr (Rat)
    {
        for (int i = 0; i < np + 1; i++)
            w = std::min(w, cpts[i].back());
    }
    double TOL = tol * w / (1 + d) + std::numeric_limits<double>::epsilon();

    // 开始移除
    int t = 0;
    for (; t < r; t++)
    {
        tmp[0] = cpts[first - 1];                   // 第一个控制点的前一个
        tmp[last + 1 - first + 1] = cpts[last + 1]; // 最后一个控制点的后一个

        // 中间向两端求解控制点方程，分别代入中间的控制点方程，继续求解
        int i = first, j = last;
        while (j - i > t)
        {
            double alphai = (u - knots[i]) / (knots[i + p + t + 1] - knots[i]);
            double alphaj = (u - knots[j - t]) / (knots[j + p + 1] - knots[j - t]);

            int ii = i - first + 1, jj = j - first + 1;
            tmp[ii] = (cpts[i] - tmp[ii - 1] * (1 - alphai)) / alphai;
            tmp[jj] = (cpts[j] - tmp[jj + 1] * alphaj) / (1 - alphaj);
            i++;
            j--;
        }

        // 判断移除计算的新控制点是否相同
        // j - i < t 则移除的控制点数为偶数，否则为奇数
        int ii = i - first + 1, jj = j - first + 1;
        if (j - i < t)
        {
            if (tmp[ii - 1].distance(tmp[jj + 1]) > TOL)
                break;
        }
        else
        {
            double alphai = (u - knots[i]) / (knots[i + p + t + 1] - knots[i]);
            if (cpts[i].distance(tmp[ii + t + 1] * alphai + tmp[ii - 1] * (1 - alphai)) > TOL)
                break;
        }

        // 更新范围控制点
        for (int i = first, j = last; j - i > t; i++, j--)
        {
            cpts[i] = tmp[i - first + 1];
            cpts[j] = tmp[j - first + 1];
        }

        first--;
        last++;
    }

    // 没有移除成功，返回 0
    if (t == 0)
        return 0;

    // 后面的节点向前移动
    for (int i = k + 1; i < nq + 1; i++)
        knots[i - t] = knots[i];

    // 找到中间控制点位置，向两边扩张，直到标记了所有需要移除的控制点范围
    int j = (k - s + k - p) / 2, i = j;
    for (int ii = 1; ii < t; ii++)
    {
        if (ii % 2 == 1)
            i++;
        else
            j--;
    }

    // 后面的控制点向前移动
    for (int ii = i + 1; ii < np + 1; ii++, j++)
        cpts[j] = cpts[ii];

    return t;
}

template <class T>
int curve_elevate(const T *cpts, const double *knots, int n, int p, int t, T *Q, double *U, int step1, int step2)
{
    int ph = p + t; // 升阶后的次数
    int ph2 = ph / 2;

    std::vector<std::vector<double>> bezAlphas(ph + 1, std::vector<double>(p + 1));
    bezAlphas[0][0] = bezAlphas[ph][p] = 1.0;

    // 计算新的控制点 P_i^t 对应的系数 alpha[i][j] * P_j
    for (int i = 1; i < ph2 + 1; i++)
    {
        double inv = 1.0 / binomial(ph, i);
        int mpi = std::min(p, i);
        for (int j = std::max(0, i - t); j < mpi + 1; j++)
            bezAlphas[i][j] = inv * binomial(p, j) * binomial(t, i - j);
    }

    // 由于二项式对称性，只需要计算一半，然后 copy 到另一半
    for (int i = ph2 + 1; i < ph; i++)
    {
        int mpi = std::min(p, i);
        for (int j = std::max(0, i - t); j < mpi + 1; j++)
            bezAlphas[i][j] = bezAlphas[ph - i][p - j];
    }

    // 记录旧的控制点，第一个控制点不用动
    Q[0] = cpts[0];

    // 旧的初始节点
    for (int i = 0; i < ph + 1; i++)
        U[i] = knots[0];

    // 保存需要处理的控制点
    std::vector<T> bpts(p + 1);
    for (int i = 0; i < p + 1; i++)
        bpts[i] = cpts[i * step1];

    int m = n - 1;     // 节点数 -1
    int mh = ph;       // 新节点数 -1
    int a = p;         // 初始从 p 开始，因此当前算法只对两端有重节点的情况有效
    int b = p + 1;     // 记录当前节点位置
    int cind = 1;      // 记录 cpts index 位置
    int kind = ph + 1; // 记录 knot index 位置
    int r = -1;        // 记录插入点数

    // 记录起始节点
    double ua = knots[0];
    while (b < m)
    {
        // 跳过重节点并计算重数
        int i = b;
        while (b < m && knots[b + 1] == knots[b])
            b++;

        int mult = b - i + 1;
        mh += mult + t; // mult 是原本的节点数，每一段节点区间都会增加 t 个新节点

        // 当前处理区间 [ua, ub] 内的控制点数
        double ub = knots[b];

        // 记录插入点数，之后要移除这些个点
        int oldr = r; // 记录之前的插入点数
        r = p - mult; // 记录现在的插入点数

        // 节点插入得到 Bezier 段，插入 ub 节点 r 次
        std::vector<T> nextBpts(r + 1);
        if (r > 0)
        {
            // 这里的处理与 curve_decompose 相同，
            std::vector<double> alphas(p - mult);
            for (int k = p; k > mult; k--)
                alphas[k - mult - 1] = (ub - ua) / (knots[a + k] - ua);

            for (int j = 1; j < r + 1; j++)
            {
                int save = r - j;
                int s = mult + j;

                // 计算新的控制点
                for (int k = p; k >= s; k--)
                    bpts[k] = bpts[k] * alphas[k - s] + bpts[k - 1] * (1 - alphas[k - s]);

                // 将当前 Bezier 段的最后一个控制点作为下一段 Bezier 的控制点
                nextBpts[save] = bpts[p];
            }
        }

        // lbz = oldr > 0 ? (oldr + 2) / 2 : 1;
        // 利用 C++ 向零取整的特性
        int lbz = oldr / 2 + 1;

        // 对当前段升阶，使用 bpts 中的控制点，保存升阶后的控制点
        std::vector<T> elevateBpts(ph + 1, T{});
        for (int i = lbz; i < ph + 1; i++)
        {
            // 这部分的处理与 bezier_elevate 相同
            int mpi = std::min(p, i);
            for (int j = std::max(0, i - t); j < mpi + 1; j++)
                elevateBpts[i] += bpts[j] * bezAlphas[i][j];
        }

        // 移除 ua 节点 oldr 次
        if (oldr > 1)
        {
            // 从中间位置 kind - 1 开始，first, last 向两端移动
            int first = kind - 2;
            int last = kind;
            double bet = (ub - U[kind - 1]) / (ub - ua);

            for (int tr = 1; tr < oldr; tr++)
            {
                int i = first, j = last;
                int kj = j - kind + 1; // 右端到中间的距离
                while (j - i > tr)
                {
                    if (i < cind)
                    {
                        double alpha = (ub - U[i]) / (ua - U[i]);
                        Q[i * step2] = Q[i * step2] * alpha + Q[(i - 1) * step2] * (1 - alpha);
                    }
                    if (j >= lbz)
                    {
                        if (j - tr <= kind - ph + oldr)
                        {
                            double gam = (ub - U[j - tr]) / (ub - ua);
                            elevateBpts[kj] = elevateBpts[kj] * gam + elevateBpts[kj + 1] * (1 - gam);
                        }
                        else
                            elevateBpts[kj] = elevateBpts[kj] * bet + elevateBpts[kj + 1] * (1 - bet);
                    }
                    i++;
                    j--;
                    kj--;
                }
                first--;
                last++;
            }
        }

        // 除了第一次循环外，都要记录 ua 节点
        if (a != p)
        {
            // 原来 ua 的重数是 ph - oldr
            for (int i = 0; i < ph - oldr; i++)
            {
                U[kind] = ua;
                kind++;
            }
        }

        // 记录升阶后的控制点
        // rbz = r > 0 ? (ph - (r + 1) / 2) : ph;
        // 利用 C++ 向零取整的特性
        int rbz = ph - (r + 1) / 2;
        for (int j = lbz; j < rbz + 1; j++)
        {
            Q[cind * step2] = elevateBpts[j];
            cind++;
        }

        // 为下一次循环做准备
        if (b < m)
        {
            for (int j = 0; j < r; j++)
                bpts[j] = nextBpts[j];
            for (int j = r; j < p + 1; j++)
                bpts[j] = cpts[(b - p + j) * step1];
            a = b;
            b++;
            ua = ub;
        }
        else
        {
            // 最后一个区间的处理，最后的控制点不用动，只需要记录 ub 重节点
            for (int i = 0; i < ph + 1; i++)
                U[kind + i] = ub;
        }
    }

    // 返回新的节点数
    return mh + 1;
}

template <class T> int curve_reduce(const T *cpts, const double *knots, int n, int p, T *Q, double *U, double tol)
{
    int ph = p - 1; // 降阶后的次数

    // 记录旧的控制点，第一个控制点不用动
    Q[0] = cpts[0];

    // 旧的初始节点
    for (int i = 0; i < ph + 1; i++)
        U[i] = knots[0];

    // 保存需要处理的控制点
    std::vector<T> bpts(p + 1);
    for (int i = 0; i < p + 1; i++)
        bpts[i] = cpts[i];

    // 保存误差
    int m = n - 1; // 节点数 -1
    std::vector<double> err(m + 1, 0);

    int mh = ph;       // 新节点数 -1
    int a = p;         // 初始从 p 开始，因此当前算法只对两端有重节点的情况有效
    int b = p + 1;     // 记录当前节点位置
    int cind = 1;      // 记录 cpts index 位置
    int kind = ph + 1; // 记录 knot index 位置
    int r = -1;        // 记录插入点数

    while (b < m)
    {
        // 跳过重节点并计算重数
        int i = b;
        while (b < m && knots[b + 1] == knots[b])
            b++;

        int mult = b - i + 1;
        mh += mult - 1; // mult 是原本的节点数，每一段节点区间都会减少 1 个新节点

        // 记录插入点数，之后要移除这些个点
        int oldr = r; // 记录之前的插入点数
        r = p - mult; // 记录现在的插入点数

        // 节点插入得到 Bezier 段，插入 ub 节点 r 次
        std::vector<T> nextBpts(r + 1);
        if (r > 0)
        {
            // 这里的处理与 curve_decompose 相同，
            std::vector<double> alphas(p - mult);
            double numer = knots[b] - knots[a];
            for (int k = p; k > mult; k--)
                alphas[k - mult - 1] = numer / (knots[a + k] - knots[a]);

            for (int j = 1; j < r + 1; j++)
            {
                int save = r - j;
                int s = mult + j;

                // 计算新的控制点
                for (int k = p; k >= s; k--)
                    bpts[k] = bpts[k] * alphas[k - s] + bpts[k - 1] * (1 - alphas[k - s]);

                // 将当前 Bezier 段的最后一个控制点作为下一段 Bezier 的控制点
                nextBpts[save] = bpts[p];
            }
        }

        // 对当前控制点降阶
        std::vector<T> rbpts(bpts.size() - 1);
        err[a] += bezier_reduce(bpts.data(), bpts.size(), rbpts.data());

        if (err[a] > tol)
            return -1;

        // lbz = oldr > 0 ? (oldr + 2) / 2 : 1;
        // 利用 C++ 向零取整的特性
        int lbz = oldr / 2 + 1;

        // 移除 ua 节点 oldr 次
        if (oldr > 0)
        {
            // 从中间位置开始，first, last 向两端移动
            int first = kind, last = kind;
            for (int k = 0; k < oldr; k++)
            {
                int i = first, j = last;
                int kj = j - kind;

                while (j - i > k)
                {
                    double alpha = (knots[a] - U[i - 1]) / (knots[b] - U[i - 1]);
                    double beta = (knots[a] - U[j - k - 1]) / (knots[b] - U[j - k - 1]);
                    Q[i - 1] = (Q[i - 1] - Q[i - 2] * (1 - alpha)) / alpha;
                    rbpts[kj] = (rbpts[kj] - rbpts[kj + 1] * beta) / (1 - beta);
                    i++;
                    j--;
                    kj--;
                }

                // 计算节点移除误差
                double Br = 0;
                if (j - i < k)
                    Br = Q[i - 2].distance(rbpts[kj + 1]);
                else
                {
                    double delta = (knots[a] - U[i - 1]) / (knots[b] - U[i - 1]);
                    auto A = rbpts[kj + 1] * delta + Q[i - 2] * (1 - delta);
                    Br = A.distance(Q[i - 1]);
                }

                // 更新误差向量
                int K = a + oldr - k;
                int q = (2 * p - k + 1) / 2;
                int L = K - q;
                for (int ii = L; ii < a + 1; ii++)
                {
                    err[ii] += Br;
                    if (err[ii] > tol)
                        return -1;
                }
                first--;
                last++;
            }
            cind = i - 1;
        }

        // 除了第一次循环外，都要记录 ua 节点
        if (a != p)
        {
            // 原来 ua 的重数是 ph - oldr
            for (int i = 0; i < ph - oldr; i++)
            {
                U[kind] = knots[a];
                kind++;
            }
        }

        // 记录降阶后的控制点
        for (int j = lbz; j < ph + 1; j++)
        {
            Q[cind] = rbpts[j];
            cind++;
        }

        // 为下一次循环做准备
        if (b < m)
        {
            for (int j = 0; j < r; j++)
                bpts[j] = nextBpts[j];
            for (int j = r; j < p + 1; j++)
                bpts[j] = cpts[b - p + j];
            a = b;
            b++;
        }
        else
        {
            // 最后一个区间的处理，最后的控制点不用动，只需要记录 ub 重节点
            for (int i = 0; i < ph + 1; i++)
                U[kind + i] = knots[b];
        }
    }

    // 返回新的节点数
    return mh + 1;
}

template <class T> std::vector<double> equally_param(const std::vector<T> &points)
{
    std::size_t n = points.size();
    std::vector<double> para(n);
    double step = 1.0 / (n - 1);
    for (std::size_t i = 0; i < n; i++)
        para[i] = step * i;
    para[n - 1] = 1.0;
    return para;
}

template <class T> std::vector<double> chord_param(const std::vector<T> &points)
{
    std::size_t n = points.size();
    std::vector<double> para(n);
    para[0] = 0;

    double total = 0;
    for (std::size_t i = 1; i < n; i++)
        total += points[i].distance(points[i - 1]);

    for (std::size_t i = 1; i < n - 1; i++)
    {
        double di = points[i].distance(points[i - 1]);
        para[i] = para[i - 1] + di / total;
    }
    para[n - 1] = 1.0;
    return para;
}

template <class T> std::vector<double> centripetal_param(const std::vector<T> &points)
{
    std::size_t n = points.size();
    std::vector<double> para(n);
    para[0] = 0;

    double total = 0;
    for (std::size_t i = 1; i < n; i++)
        total += std::sqrt(points[i].distance(points[i - 1]));

    for (std::size_t i = 1; i < n - 1; i++)
    {
        double di = std::sqrt(points[i].distance(points[i - 1]));
        para[i] = para[i - 1] + di / total;
    }
    para[n - 1] = 1.0;
    return para;
}

template <class T> std::vector<std::vector<vec2>> equally_param(const std::vector<std::vector<T>> &points)
{
    std::vector<std::vector<vec2>> para(points.size(), std::vector<vec2>(points[0].size()));

    double ustep = 1.0 / (points.size() - 1);
    double vstep = 1.0 / (points[0].size() - 1);

    for (std::size_t i = 0; i < points.size(); i++)
    {
        double u = ustep * i;
        for (std::size_t j = 0; j < points[0].size(); j++)
        {
            double v = vstep * j;
            para[i][j] = vec2{u, v};
        }
        para[i].back() = vec2{u, 1.0};
    }
    para.back().back() = vec2{1.0, 1.0};
    return para;
}

template <class T> std::vector<std::vector<vec2>> chord_param(const std::vector<std::vector<T>> &points)
{
    std::vector<std::vector<vec2>> para(points.size(), std::vector<vec2>(points[0].size()));
    for (std::size_t i = 0; i < points.size(); i++)
    {
        double total = 0;
        for (std::size_t j = 1; j < points[0].size(); j++)
            total += points[i][j].distance(points[i][j - 1]);

        para[i][0][1] = 0;
        for (std::size_t j = 1; j < points[0].size(); j++)
        {
            double di = points[i][j].distance(points[i][j - 1]);
            para[i][j][1] = para[i][j - 1][1] + di / total;
        }
        para[i].back()[1] = 1.0;
    }

    for (std::size_t i = 0; i < points[0].size(); i++)
    {
        double total = 0;
        for (std::size_t j = 1; j < points.size(); j++)
            total += points[j][i].distance(points[j - 1][i]);

        para[0][i][0] = 0;
        for (std::size_t j = 1; j < points.size(); j++)
        {
            double di = points[j][i].distance(points[j - 1][i]);
            para[j][i][0] = para[j - 1][i][0] + di / total;
        }
        para.back()[i][0] = 1.0;
    }
    return para;
}

template <class T> std::vector<std::vector<vec2>> centripetal_param(const std::vector<std::vector<T>> &points)
{
    std::vector<std::vector<vec2>> para(points.size(), std::vector<vec2>(points[0].size()));
    for (std::size_t i = 0; i < points.size(); i++)
    {
        double total = 0;
        for (std::size_t j = 1; j < points[0].size(); j++)
            total += std::sqrt(points[i][j].distance(points[i][j - 1]));

        para[i][0][1] = 0;
        for (std::size_t j = 1; j < points[0].size(); j++)
        {
            double di = std::sqrt(points[i][j].distance(points[i][j - 1]));
            para[i][j][1] = para[i][j - 1][1] + di / total;
        }
        para[i].back()[1] = 1.0;
    }

    for (std::size_t i = 0; i < points[0].size(); i++)
    {
        double total = 0;
        for (std::size_t j = 1; j < points.size(); j++)
            total += std::sqrt(points[j][i].distance(points[j - 1][i]));

        para[0][i][0] = 0;
        for (std::size_t j = 1; j < points.size(); j++)
        {
            double di = std::sqrt(points[j][i].distance(points[j - 1][i]));
            para[j][i][0] = para[j - 1][i][0] + di / total;
        }
        para.back()[i][0] = 1.0;
    }
    return para;
}

std::vector<double> equally_knots(std::size_t n, std::size_t p)
{
    std::vector<double> knots(n + p + 1);
    for (std::size_t i = 1; i <= p + 1; i++)
        knots[i - 1] = 0;
    double step = 1.0 / (n - p);
    for (std::size_t i = p + 2; i <= n; i++)
        knots[i - 1] = step * (i - p - 1);
    for (std::size_t i = n + 1; i <= n + p + 1; i++)
        knots[i - 1] = 1;
    return knots;
}

std::vector<double> average_knots(const std::vector<double> &params, std::size_t p)
{
    // 注意确保参数值归一化
    std::size_t n = params.size();
    std::vector<double> knots(n + p + 1);
    for (std::size_t i = 1; i <= p + 1; i++)
        knots[i - 1] = 0;

    for (std::size_t i = p + 2; i <= n; i++)
    {
        double s = 0;
        for (std::size_t j = 1; j <= p; j++)
            s += params[i - p - 2 + j];
        knots[i - 1] = s / p;
    }

    for (std::size_t i = n + 1; i <= n + p + 1; i++)
        knots[i - 1] = 1;
    return knots;
}

std::vector<double> uniform_knots(const std::vector<double> &params, std::size_t n, std::size_t p)
{
    // 注意确保参数值归一化
    std::size_t m = params.size();
    double d = 1.0 * m / (n - p);
    std::vector<double> knots(n + p + 1);
    for (std::size_t i = 1; i <= p + 1; i++)
        knots[i - 1] = 0;

    for (std::size_t j = 1; j < n - p; j++)
    {
        int i = j * d;
        double alpha = j * d - i;
        knots[j + p] = params[i - 1] * (1 - alpha) + params[i] * alpha;
    }

    for (std::size_t i = n + 1; i <= n + p + 1; i++)
        knots[i - 1] = 1;
    return knots;
}

} // namespace geom

} // namespace xy
